import numpy as np
from scipy.optimize import minimize
import cv2

# 定义损失函数
def loss_function(params, x, y):
    predicted_y = params[0] * np.sin(x) + params[1] * np.cos(x) + params[2]
    return np.sum((predicted_y - y)**2)

# 打开并读取config.yaml文件
data_file = cv2.FileStorage('config\config.yaml', cv2.FILE_STORAGE_READ)

# 从文件中获取x_data和y_data
num = int(data_file.getNode('num').real())
x_data = []
y_data = []
# print(num)
for i in range(num):
    x_data.append(float(data_file.getNode('theta_' + str(i)).real()))
    y_data.append(data_file.getNode('observed_T_' + str(i)).mat())

y_data1 = [y[0][0] for y in y_data]
y_data2 = [y[1][0] for y in y_data]
y_data3 = [y[2][0] for y in y_data]

# 初始化参数
initial_params1 = np.ones(3)
initial_params2 = np.ones(3)
initial_params3 = np.ones(3)

# 使用cyipy.optimize.minimize函数进行优化
result1 = minimize(loss_function, initial_params1, args=(x_data, y_data1), method='L-BFGS-B')
result2 = minimize(loss_function, initial_params2, args=(x_data, y_data2), method='L-BFGS-B')
result3 = minimize(loss_function, initial_params3, args=(x_data, y_data3), method='L-BFGS-B')

# 输出优化得到的参数
# print("Optimal parameters:", result1.x)
# print("Optimal parameters:", result2.x)
# print("Optimal parameters:", result3.x)

# 根据优化的参数，计算拟合后的y_data
y_data1_fit = []
y_data2_fit = []
y_data3_fit = []
for x in x_data:
    y_data1_fit.append(result1.x[0] * np.sin(x) + result1.x[1] * np.cos(x) + result1.x[2])
    y_data2_fit.append(result2.x[0] * np.sin(x) + result2.x[1] * np.cos(x) + result2.x[2])
    y_data3_fit.append(result3.x[0] * np.sin(x) + result3.x[1] * np.cos(x) + result3.x[2])

print(y_data1_fit)
print(y_data2_fit)
print(y_data3_fit)

# 画图，x从-2pi到0
# import matplotlib.pyplot as plt
# x = np.linspace(-2*np.pi, 0, 100)
# plt.figure()
# plt.plot(x_data, y_data1, 'o', label='Original data')
# plt.plot(x, result1.x[0] * np.sin(x) + result1.x[1] * np.cos(x) + result1.x[2], label='Fitted line')
# plt.legend()
# plt.show()
# plt.figure()
# plt.plot(x_data, y_data2, 'o', label='Original data')
# plt.plot(x, result2.x[0] * np.sin(x) + result2.x[1] * np.cos(x) + result2.x[2], label='Fitted line')
# plt.legend()
# plt.show()
# plt.figure()
# plt.plot(x_data, y_data3, 'o', label='Original data')
# plt.plot(x, result3.x[0] * np.sin(x) + result3.x[1] * np.cos(x) + result3.x[2], label='Fitted line')
# plt.legend()
# plt.show()

# 和原标定法进行对比
tie = [0.0431247, 0.00903383, -0.0100873]
tec = [0.0211704, -0.00364647, -0.00988826]
a = [0.00177999, -0.0098217, 0.99995]

def skew_symmetric_matrix(a):
    return np.array([[0, -a[2], a[1]],
                     [a[2], 0, -a[0]],
                     [-a[1], a[0], 0]])

def calculate_vector(x, a, tie, tec):
    a_hat = skew_symmetric_matrix(a)
    return tie + np.exp(x * a_hat) @ tec

# 初始化结果列表
result_vectors = []

# 计算每个x对应的向量

for x in x_data:
    result_vectors.append(calculate_vector(-x, a, tie, tec))

# 打印结果向量
print(x_data)
for vector in result_vectors:
    print(vector)